import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory, get_package_prefix
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.substitutions import LaunchConfiguration


workspace = os.path.split(get_package_prefix('imgprocess'))[0]
workspace = os.path.split(workspace)[0]


# 获得config的路径
config = os.path.join(
    workspace,
    'src',
    'config'
)
car_id = os.getenv('CAR_ID', '3')

# config下的文件路径
g_camera_param_path = os.path.join(config, 'camera_param'+car_id+'.xml')
g_mv_camera_config_path = os.path.join(
    config, 'mv_camera_config'+car_id+'.yaml')
g_tune_param_path = os.path.join(config, 'tune_param'+car_id+'.yaml')
g_other_param_path = os.path.join(config, 'other_param.xml')

use_sim_time = LaunchConfiguration('use_sim_time', default='true')


def generate_launch_description():
    # """launch内容描述函数，由ros2 launch 扫描调用"""
    # 声明为launch参数
    # print(workspace)
    declare_workspace_path = DeclareLaunchArgument(
    'workspace_path',
    default_value=workspace,
    description='')
    declare_g_camera_param_path = DeclareLaunchArgument(
        'g_camera_param_path',
        default_value=g_camera_param_path,
        description='')
    declare_g_mv_camera_config_path = DeclareLaunchArgument(
        'g_mv_camera_config_path',
        default_value=g_mv_camera_config_path,
        description=''
    )
    declare_g_tune_param_path = DeclareLaunchArgument(
        'g_tune_param_path',
        default_value=g_tune_param_path,
        description=''
    )
    declare_g_other_param_path = DeclareLaunchArgument(
        'g_other_param_path',
        default_value=g_other_param_path,
        description=''
    )
    declare_car_id = DeclareLaunchArgument(
        'car_id',
        default_value=car_id,
        description=''
    )
    
    workspace_path_arg = LaunchConfiguration('workspace_path')
    car_id_arg = LaunchConfiguration('car_id')
    g_camera_param_path_arg = LaunchConfiguration('g_camera_param_path')
    g_mv_camera_config_path_arg = LaunchConfiguration(
        'g_mv_camera_config_path')
    g_tune_param_path_arg = LaunchConfiguration('g_tune_param_path')
    g_other_param_path_arg = LaunchConfiguration('g_other_param_path')
    path_para = {"workspace_path": workspace_path_arg,
    		"g_camera_param_path": g_camera_param_path_arg,
                 "g_mv_camera_config_path": g_mv_camera_config_path_arg,
                 "g_tune_param_path": g_tune_param_path_arg,
                 'g_other_param_path': g_other_param_path_arg,
                 'car_id': car_id_arg}

    return LaunchDescription([
        #  use_sim_time = LaunchConfiguration('use_sim_time', default='true'),
        declare_workspace_path,
        declare_g_camera_param_path,
        declare_g_mv_camera_config_path,
        declare_g_tune_param_path,
        declare_g_other_param_path,
        declare_car_id,
        Node(
            package="autoaim",
            executable="auto_aim",
            output='both',
            parameters=[path_para])
    ])
